Mark -

Here's a try at "repairing" the code.  I think this method will scale to
any number of joints, as long as you don't go crazy!

I think the only difference is that I am figuring out how much each
joint should move by adding up two vector functions and weighting them
by a factor ("contraction").  This seems like a pretty general method.  

I would like to add in "stiffness" to the joints that responds to the
MinAng and MaxAng parameters.

Anyway, it still needs to be cleaned up a bit but I hope it works for
you.


- Ben

>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

#debug "\n\nStarting IK Finger...\n\n"

//Some containers for numbers
#declare Length = array[3]{2,2,2}
#declare Rotate = array[3]{0,0,0}
#declare MinAng = array[3]{-20,-90,-2}
#declare MaxAng = array[3]{90,90,90}


//Try both of these joint definitions!!!
#declare Joint  = array[3]{<1,0,0>, <0,1,0>, <0,0,1>}
//#declare Joint  = array[3]{<0,0,1>, <0,0,1>, <0,0,1>}


#declare tJoint = array[3]{<0,0,1>, <0,1,0>, <0,0,1>}

#declare Base   = array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Tip    = array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Torque = array[3]{0,0,0}

#declare seed1 = seed(-1);

//
//  Important Constants
//

#declare TargetLocation = <2,3,0>;
#declare FingerLocation = <0,0,0>;

#declare Delta = 1.0;
#declare TargetBailout=0.3;
#declare TorqueBailout=0.001;
#declare MaxIterations=800;
#declare TargetReached=no;

#declare contraction    = 1/1;



//
//  Define the workhorse functions to 
//  calculate angular and radial "distances"
//

#macro DistAng(VA,VB)
(  1.0  -  vdot(VA,VB)/vlength(VA)/vlength(VB) )/2.0
#end

#macro DistRad(V1,V2)
(  vlength(V1)  -  vlength(V2)  )
#end

#macro PlaneNormal(V1,V2)
vnormalize( vcross(V1,V2) )
#end

#macro WeightAng(Target,Tip,Joint)
DistAng(Target,Tip)*vdot(PlaneNormal(Tip,Target),Joint)
#end

//random numbers added so the program wouldn't freeze up
//if the joints were in a straight line
#macro WeightRad(Target,Tip,Leg,Joint)
(
DistRad(Target,Tip)*vdot(PlaneNormal(Leg,Tip-Leg),Joint)/vlength(Target)
+ (rand(seed1)-0.5)*1e-5 )
#end

//
//Some tools
//
#macro ShowVector(V1, V2)
  cone{V1, 0.05, V2, 0.0 texture{pigment{color rgb<1,1,0>}}}
#end

#macro Vector2Text(V)
  vstr(3,V,", ",4,2)
#end

#macro Number2Text(N)
  str(N,7,4)
#end

//
//  Define Common Objects
//

#macro MakeJoint(V1,V2)
  merge{
    sphere{V1, 0.2}
    cone{V1, 0.2, V2, 0.05}
    sphere{V2, 0.05}
    cylinder{V1, V2 + 50*(V2-V1), 0.01}
  }
#end

#declare Target=
  union{
    cone{0.06*x, 0, 1*x, 0.2}
    cone{0.06*x, 0, 1*x, 0.2 rotate 90*z}
    cone{0.06*x, 0, 1*x, 0.2 rotate 180*z}
    cone{0.06*x, 0, 1*x, 0.2 rotate 270*z}
    sphere{0, TargetBailout}
    texture{pigment{color rgbt<0,1,1,0.5>}}
  }


#if (clock_on)
  #declare current_frame=(clock/clock_delta)+1;
#else
  #declare current_frame=1;
#end
  #debug concat("\n ******* ", Number2Text(current_frame), " *******\n")

//Calculates positions of each tip, and if the last one is close to the
target,
//  sets a variable to say so.
#macro CalcTipsFK()
  #declare Base[0]   = FingerLocation;
  #declare tJoint[0] = Joint[0];
  #declare Tip[0]    = Base[0] +
vaxis_rotate(Length[0]*x,tJoint[0],Rotate[0]);

  #declare Base[1]   = Tip[0];
  #declare tJoint[1] =           vaxis_rotate(Joint[1]  
,tJoint[0],Rotate[0]);
  #declare Tip[1]    =          
vaxis_rotate(Length[1]*x,tJoint[0],Rotate[0]);
  #declare Tip[1]    = Base[1] + vaxis_rotate(Tip[1]    
,tJoint[1],Rotate[1]);
  
  #declare Base[2]   = Tip[1];
  #declare tJoint[2] =           vaxis_rotate( Joint[2] 
,tJoint[0],Rotate[0]);
  #declare tJoint[2] =           vaxis_rotate(tJoint[2] 
,tJoint[1],Rotate[1]);
  #declare Tip[2]    =          
vaxis_rotate(Length[2]*x,tJoint[0],Rotate[0]);
  #declare Tip[2]    =           vaxis_rotate(Tip[2]    
,tJoint[1],Rotate[1]);
  #declare Tip[2]    = Base[2] + vaxis_rotate(Tip[2]    
,tJoint[2],Rotate[2]);
  
  #if (vlength(TargetLocation-Tip[2])<TargetBailout)
    #declare TargetReached=yes;
    #debug "\n**Target Reached"
    #debug concat("\n  Iterations: ", Number2Text(ct))

  #else
    #declare TargetReached=no;
  #end
#end


//rotate the joint to point at the target
#macro ReorientTip2()
  CalcTipsFK()

  #declare TotLength = Length[0]+Length[1]+Length[2];
   
  #declare ct = 0;
  #declare finished = false;
  #while (!finished & ct<MaxIterations)
    #if (TargetReached)
      #declare finished = true;
      #debug "TR"
    #else
      CalcTipsFK()

      #declare TotTip    = Tip[2]-Base[0];
      
      #declare tTarget   = TargetLocation-Base[0];
      #declare tTip      = Tip[2]-Base[0];
      #declare tLegLast  = x;
      #declare tLeg      = Tip[0]-Base[0];
      #declare tLength   = Length[0]+Length[1]+Length[2];
      #declare Torque[0] = ( WeightAng(tTarget,tTip,Joint[0]) + 
                            
0*WeightRad(tTarget,TotTip,tLeg,Joint[0])*contraction ); 
                                                 
      #declare tTarget   = TargetLocation-Base[1];         
      #declare tTip      = Tip[2]-Base[1];
      #declare tLegLast  = tLeg;
      #declare tLeg      = Tip[1]-Base[1];
      #declare tLength   = Length[1]+Length[2];
      #declare Torque[1] = ( 0*WeightAng(tTarget,tTip,Joint[1]) + 
                            
WeightRad(tTarget,TotTip,tLeg,Joint[1])*contraction );

      #declare tTarget   =
TargetLocation-Base[2];                           
      #declare tTip      = Tip[2]-Base[2];
      #declare tLegLast  = tLeg;
      #declare tLeg      = Tip[2]-Base[2];
      #declare tLength   = Length[2];
      #declare Torque[2] = ( WeightAng(tTarget,tTip,Joint[2]) + 
                            
0*WeightRad(tTarget,TotTip,tLeg,Joint[2])*contraction );
      
      #declare Weight = abs(Torque[0]) + abs(Torque[1]) + abs(Torque[2])
+ 1e-5;
      //Put in the small constant so there wouldn't be any "divide by
zero" errors
      
      #declare Rotate[0] = max(Rotate[0] + Delta * Torque[0] / Weight,
MinAng[0]);
      #declare Rotate[1] = max(Rotate[1] + Delta * Torque[1] / Weight,
MinAng[1]);
      #declare Rotate[2] = max(Rotate[2] + Delta * Torque[2] / Weight,
MinAng[2]);
      
      #declare Rotate[0] = min( Rotate[0], MaxAng[0] );
      #declare Rotate[1] = min( Rotate[1], MaxAng[1] );
      #declare Rotate[2] = min( Rotate[2], MaxAng[2] );
      
      #declare ct = ct + 1;

      #debug concat("\n  Rotate[0]: ", Number2Text(Rotate[0]))
      #debug concat("    Rotate[1]: ", Number2Text(Rotate[1]))
      #debug concat("    Rotate[2]: ", Number2Text(Rotate[2]))
      
      //#debug concat("\n  Iterations: ", Number2Text(ct))
      //#debug concat("\n  Torque[2]: ", Number2Text(Torque[2]/Delta))
    #end
  #end
#end


ReorientTip2()
CalcTipsFK()

//
//  Somehow, these declarations needed to be moved to work...
//

#declare Joint0 = 
object{
        MakeJoint(FingerLocation,Tip[0])
        texture{pigment{color rgbt<1,0,0,0.5>}}
}
#declare Joint1 = 
object{
        MakeJoint(Tip[0],Tip[1])
        texture{pigment{color rgbt<0,1,0,0.5>}}
}
#declare Joint2 = object{
        MakeJoint(Tip[1],Tip[2])
        texture{pigment{color rgbt<0,0,1,0.5>}}
}

object{Target translate TargetLocation}
object{Joint0}
object{Joint1}
object{Joint2}


//
//My stupid version of your "robot-testrig.inc" file
//

background{ rgb <1,1,1> }

#declare Camera_Location=<3/2, 3/2, -20>;
#declare Camera_Target=<3/2,3/2,0>;
#declare Camera_Angle=25;
#declare Use_AreaLights=no;
#declare Use_Rad=no;
#declare Ground_Height=-1;
#declare Draw_Sky=no;
#declare Draw_Ground=no;


// create a regular point light source
light_source
{
  0*x // light's position (translated below)
  color red 1.0  green 1.0  blue 1.0  // light's color
  translate <-20, 40, -20>
}

camera {
  location  Camera_Location
  look_at   Camera_Target
  angle     Camera_Angle
}


//#include "robot-testrig.inc"

#debug "\n\nEnd\n\n"

Post a reply to this message